/*
 * RotateBehavior.cc
 *
 */

#include "RotateBehavior.hh"

RotateBehavior::RotateBehavior(const sense::State& s,
                               const sense::Environment& e, sense::Command& c, std::string cf) :
        BehaviorSafe(s, e, c, cf) {

    // Read parameters from configuration file
    util::ConfigFile conf(cf);
    rot_vel_num=	conf.getDouble("ROT_VEL_NUM");;
    rot_vel_via=	conf.getDouble("ROT_VEL_VIA");;
}

void RotateBehavior::actSafe() {

    if (state.numbers.empty() && mode==1) {
        std::cout << "ROTATE accazzum"  << std::endl;

        command.linVel = 0;
	if (farthestPoint>2)
	  command.rotVel = 0.7;//(rot_vel_num * farthestPoint);
	else 
	  command.rotVel=rot_vel_num;
    }
    if (mode==2 && farthestPoint < 3) {  //secondo lollo quel farthestPoint non serve a nulla
        std::cout << "ROTATE fase vai via"  << std::endl;

        command.linVel = 0;
        command.rotVel = rot_vel_via; //può andare più velox! stagno
    }
    return;
}
